import os
import sys
sys.path.append("..")
import numpy as np
from transforms3d.quaternions import mat2quat, quat2mat


def lerp(p0, p1, alpha):
    """
    alpha * p0 + (1 - alpha) * p1
    """
    
    assert p0.shape == (4, 4)
    assert p1.shape == (4, 4)
    
    R0 = p0[:3, :3]
    t0 = p0[:3, 3:]
    R1 = p1[:3, :3]
    t1 = p1[:3, 3:]
    
    q = alpha * mat2quat(R0) + (1 - alpha) * mat2quat(R1)
    q /= np.linalg.norm(q, ord=2)
    R = quat2mat(q)
    t = alpha * t0 + (1 - alpha) * t1
    
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3:] = t
    
    return T
